#include "drv_pwmMotors.h"
#include "stdbool.h"
#include "tim.h"
#include "math.h"
#define MAX_CNT (PWM_PERIOD * 8 / 10)  
short int sinDataI16[SINARRAYSIZE];
/****************************************************************

函数声明

****************************************************************/
void Motor_RotorPosition(ZY_motor_drive_t *motor_tcb);
void Motor_Mixer_RotorPosition(ZY_motor_drive_t *motor_tcb);
void ZY_Set_Power_Motor(ZY_motor_drive_t *motor_drive, float angle, float power);
float _Round(float x);
void ZY_initSinArray(void);
/****************************************************
功能：电机初始化
参数：无
返回：无
****************************************************/
void ZY_Motor_Init(ZY_motor_drive_t *motor_tcb,uint8_t pole_paris,float power,Motor_C start_pwm,Motor_U set_pwm)
{
	motor_tcb->pwm_drive.pwm_en_dn = enable_pwm;
	motor_tcb->Pole_Paris = pole_paris;
	motor_tcb->MaxPower = power;
	motor_tcb->LeadAngle = (motor_tcb->encoder.value.Resolution / motor_tcb->Pole_Paris) * 2.0 / TWOPI;
	motor_tcb->motor_power = 0;
	motor_tcb->EnableStop = false;
	motor_tcb->pwm_drive.start_pwm=start_pwm;   
	motor_tcb->pwm_drive.set_pwm=set_pwm;
	motor_tcb->pwm_drive.start_pwm();  //启动PWM
}

/**************************************************
*功能：电机转子位置计算
*参数：无
*返回：无
*修改记录：     
*                2020 07.21  张亚辉 创建电机转子位置控制函数
**************************************************/
void ZY_Motor_RotorPosition(ZY_motor_drive_t *motor_tcb)
{
	uint32_t rotor = 0;
	float advance_angle;
	motor_tcb->motor_power = fabs(motor_tcb->motor_power) > motor_tcb->MaxPower ? SYBOLNUM(motor_tcb->motor_power) * motor_tcb->MaxPower : motor_tcb->motor_power;
	if(!motor_tcb->EnableStop)
	{
		if(motor_tcb->motor_power > 0)
		{
			rotor = motor_tcb->encoder.user_value + motor_tcb->LeadAngle;
			if(rotor > motor_tcb->encoder.value.Resolution)
				rotor -= (motor_tcb->encoder.value.Resolution + 1);
		}
		else
		{
			rotor = (motor_tcb->encoder.user_value - motor_tcb->LeadAngle) & motor_tcb->encoder.value.Resolution;
		}
		advance_angle = ((float)rotor / (float)(motor_tcb->encoder.value.Resolution)) * motor_tcb->Pole_Paris * TWOPI;
		ZY_Set_Power_Motor(motor_tcb,advance_angle,(int)fabs(motor_tcb->motor_power));
	}
	else
	{
		rotor = motor_tcb->encoder.user_value;
		//advance_angle = ((float)rotor / (float)(motor_drive->encoder.value->Resolution)) * motor_drive->Pole_Paris * TWOPI;
		//_Set_Power_Motor(&motor_drive->pwm_drive,advance_angle,(int)fabs(motor_drive->motor_power));
	}
	if(motor_tcb->encoder.value.status == 1)
	{
		ZY_Set_Power_Motor(motor_tcb,advance_angle,0);
	}
}

/*********************************************************
 * 
 * ******************************************************/
void Motor_Mixer_RotorPosition(ZY_motor_drive_t *motor_tcb)
{
	uint32_t rotor = 0;
	float advance_angle;
	motor_tcb->motor_power = fabs(motor_tcb->motor_power) > motor_tcb->MaxPower ? SYBOLNUM(motor_tcb->motor_power) * motor_tcb->MaxPower : motor_tcb->motor_power;
	if(!motor_tcb->EnableStop)
	{
		if(motor_tcb->motor_power > 0)
		{
			rotor = (uint32_t)(motor_tcb->rotor + motor_tcb->LeadAngle);
			if(rotor > motor_tcb->encoder.value.Resolution)
				rotor -= (motor_tcb->encoder.value.Resolution + 1);
		}
		else
		{
			rotor = ((uint32_t)(motor_tcb->rotor - motor_tcb->LeadAngle)) & motor_tcb->encoder.value.Resolution;
		}
		advance_angle = ((float)rotor / (float)(motor_tcb->encoder.value.Resolution)) * motor_tcb->Pole_Paris * TWOPI;
		ZY_Set_Power_Motor(motor_tcb,advance_angle,(int)fabs(motor_tcb->motor_power));
	}
	else
	{
		ZY_Set_Power_Motor(motor_tcb,advance_angle,(int)fabs(motor_tcb->motor_power));
		//_Set_Power_Motor(&motor_drive->pwm_drive,advance_angle,55);
	}
	if(motor_tcb->encoder.value.status == 1)
	{
		ZY_Set_Power_Motor(motor_tcb,advance_angle,0);
	}
}

/*******************************************************
功能：motor设置功率
参数：*pwm_drive
		angle：相位角
		power：幅度
返回：无
********************************************************/
void ZY_Set_Power_Motor(ZY_motor_drive_t *motor_tcb, float angle, float power)
{
	int iPower;
	int angleInt;
	angleInt = (int)_Round(angle / TWOPI * SINARRAYSIZE);
	angleInt = angleInt % SINARRAYSIZE;
	if (angleInt < 0)
  {
		angleInt = SINARRAYSIZE + angleInt;
  }
	iPower = (int)((PWM_PERIOD / 2) * power / 100);
    motor_tcb->pwm_drive.Phase[0] = (sinDataI16[ angleInt                               % SINARRAYSIZE] * iPower + SINARRAYSCALE / 2) / SINARRAYSCALE + PWM_PERIOD / 2;
    motor_tcb->pwm_drive.Phase[1] = (sinDataI16[(angleInt +  1 * SINARRAYSIZE / 3)      % SINARRAYSIZE] * iPower + SINARRAYSCALE / 2) / SINARRAYSCALE + PWM_PERIOD / 2;
    motor_tcb->pwm_drive.Phase[2] = (sinDataI16[(angleInt + (2 * SINARRAYSIZE + 1) / 3) % SINARRAYSIZE] * iPower + SINARRAYSCALE / 2) / SINARRAYSCALE + PWM_PERIOD / 2;
	
	  if(motor_tcb->pwm_drive.pwm_en_dn == enable_pwm)
		{
			motor_tcb->pwm_drive.set_pwm(motor_tcb->pwm_drive.Phase[0],motor_tcb->pwm_drive.Phase[1],motor_tcb->pwm_drive.Phase[2]);
		}
		else
		{
			motor_tcb->pwm_drive.set_pwm(0,0,0);
		}
		//HAL_TIM_Base_Start_IT(motor_tcb->pwm_drive.htim);
}
//static void _Set_Power_Motor(pwm_drive_s *pwm_drive, float angle, float power)
//{
//	int iPower;
//	int angleInt;
//	if(power<=0.0)
//  {
//	  pwm_drive->Phase[0]=0;
//		pwm_drive->Phase[1]=0;
//		pwm_drive->Phase[2]=0;
//		return;
//	}
//	angleInt = (int)_Round(angle / TWOPI * SINARRAYSIZE);
//	angleInt = angleInt % SINARRAYSIZE;
//  if (angleInt < 0)
//  {
//		angleInt = SINARRAYSIZE + angleInt;
//  }
//	iPower = (int)((float)PWM_PERIOD*power/ 100.0);
//	if(angleInt<1364)
//	{
//    pwm_drive->Phase[0] =sinDataI16[angleInt]*iPower/SINARRAYSCALE;//sin(angle)
//    pwm_drive->Phase[2] =0;
//    pwm_drive->Phase[1] =sinDataI16[1364-angleInt]*iPower/SINARRAYSCALE;//-sin(angle-120)=sin(120-angle),for look_up table
//	}
//	else if(angleInt<2728)
//	{
//    pwm_drive->Phase[0] =sinDataI16[2728-angleInt]*iPower/SINARRAYSCALE;//-sin(angle-240)=sin(240-angle)),for look_up table
//    pwm_drive->Phase[2] =sinDataI16[angleInt-1364]*iPower/SINARRAYSCALE;//sin(angle-120)
//    pwm_drive->Phase[1] =0;
//	}
//	else
//	{
//    pwm_drive->Phase[0] =0;
//    pwm_drive->Phase[2] =-sinDataI16[angleInt]*iPower/SINARRAYSCALE;//-sin(angle)
//    pwm_drive->Phase[1] =sinDataI16[angleInt-2728]*iPower/SINARRAYSCALE;//sin(angle-240)
//	}
//	__HAL_TIM_ENABLE_IT(pwm_drive->htim, TIM_IT_UPDATE);
//}

void ZY_SetMotorPWM(ZY_motor_drive_t *motor_tcb)
{
	uint16_t a=0,b=0,c=0;
	if(motor_tcb->pwm_drive.pwm_en_dn == enable_pwm)
	{
		a=motor_tcb->pwm_drive.Phase[0];
		b=motor_tcb->pwm_drive.Phase[1];
		c=motor_tcb->pwm_drive.Phase[2];
	}
	//motor_tcb->pwm_drive.updata_pwm(a,b,c);
}
/****************************************************
功能：设置定时器三个通道PWM值
参数：motor_drive
返回：无
****************************************************/
#define MAX_CNT (PWM_PERIOD * 8 / 10)
/****************************************************
功能：计算三角周期
参数：x
返回：无
****************************************************/
static float _Round(float x)
{
    if (x >= 0)
    {
        return x + 0.5F;
    }
    else
    {
        return x - 0.5F;
    }
}
/****************************************************
功能：计算正弦查询表
参数：无
返回：无
****************************************************/
void ZY_initSinArray(void)
{
    int i;
    for (i = 0; i < SINARRAYSIZE; i++)
    {
        float x = i * TWOPI / SINARRAYSIZE;
        sinDataI16[i] = (short int)_Round(sinf(x)*fabs(sinf(x)) * SINARRAYSCALE);
    }
}


